<!-- saved from url=(0014)about:internet -->
<html>
<body>
<script src="resources/eml_report_loadable_data.js"></script>
<pre class="code">
<span class="srcline"><span class="lineno"><a href="1,1" id="srcline1"> 1</a></span><span class="line"><span class="keyword">function</span> [<span class="var type1" id="S2T1U3">x_d</span>,<span class="var type1" id="S3T1U4">y_d</span>,<span class="var type1" id="S4T1U5">z_d</span>,<span class="var type0" id="S5T0U6">yaw_d</span>] = fcn(<span class="var type1" id="S6T2U9">desired</span>,<span class="var type1" id="S7T1U10">act_x</span>,<span class="var type1" id="S8T1U11">act_y</span>,<span class="var type1" id="S9T1U12">act_z</span>,<span class="var type1" id="S10T3U13">act_yaw</span>,<span class="var type1" id="S11T3U14">v_vector</span>,<span class="var type1" id="S12T1U15">w_yaw</span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,2" id="srcline2"> 2</a></span><span class="line"><span class="keyword">global</span> <span class="var type0" id="S13T0U17">i</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,3" id="srcline3"> 3</a></span><span class="line"><span class="keyword">global</span> <span class="var type0" id="S14T0U19">finally_flag</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,4" id="srcline4"> 4</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,5" id="srcline5"> 5</a></span><span class="line"><span class="comment">% desired_pos_series = [0,0,10;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,6" id="srcline6"> 6</a></span><span class="line"><span class="comment">%                       5,0,10;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,7" id="srcline7"> 7</a></span><span class="line"><span class="comment">%                       5,5,10;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,8" id="srcline8"> 8</a></span><span class="line"><span class="comment">%                       0,5,10;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,9" id="srcline9"> 9</a></span><span class="line"><span class="comment">%                       0,0,10];%期望路径x,y,z(z是正的表示高度)</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,10" id="srcline10">10</a></span><span class="line"><span class="comment">%判断误差，当满足误差条件的时候，再给出下一个点，不满足时还是上一个点</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,11" id="srcline11">11</a></span><span class="line"><span class="mxinfo " id="T1:U11"><span class="var type1" id="S15T1U22">error_x</span> = <span class="mxinfo " id="T1:U13"><span class="mxinfo " id="T1:U14"><span class="var type1" id="S6T2U25">desired</span>(<span class="var type1" id="S13T1U26">i</span>,<span class="mxinfo " id="T4:U17">1</span>)</span> - <span class="var type1" id="S7T1U28">act_x</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,12" id="srcline12">12</a></span><span class="line"><span class="mxinfo " id="T1:U19"><span class="var type1" id="S16T1U31">error_y</span> = <span class="mxinfo " id="T1:U21"><span class="mxinfo " id="T1:U22"><span class="var type1" id="S6T2U34">desired</span>(<span class="var type1" id="S13T1U35">i</span>,<span class="mxinfo " id="T1:U25">2</span>)</span> - <span class="var type1" id="S8T1U37">act_y</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,13" id="srcline13">13</a></span><span class="line"><span class="mxinfo " id="T1:U27"><span class="var type1" id="S17T1U40">error_z</span> = <span class="mxinfo " id="T1:U29"><span class="mxinfo " id="T1:U30"><span class="var type1" id="S6T2U43">desired</span>(<span class="var type1" id="S13T1U44">i</span>,<span class="mxinfo " id="T1:U33">3</span>)</span> - <span class="mxinfo " id="T1:U34">abs(<span class="var type1" id="S9T1U48">act_z</span>)</span></span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="1,14" id="srcline14">14</a></span><span class="line"><span class="var type1" id="S13T1U50">i</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,15" id="srcline15">15</a></span><span class="line"><span class="mxinfo " id="T1:U37"><span class="var type1" id="S19T1U53">error_all</span> = <span class="mxinfo " id="T1:U39"><span class="mxinfo " id="T1:U40"><span class="var type1" id="S15T1U56">error_x</span>+<span class="var type1" id="S16T1U57">error_y</span></span>+<span class="var type1" id="S17T1U58">error_z</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,16" id="srcline16">16</a></span><span class="line"><span class="mxinfo " id="T5:U44"><span class="autoExtrinsicFcn" id="F6N2:B61">disp</span>(<span class="var type1" id="S19T1U62">error_all</span>)</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,17" id="srcline17">17</a></span><span class="line"><span class="keyword">if</span> ((<span class="mxinfo " id="T6:U46"><span class="mxinfo " id="T1:U47">abs(<span class="var type1" id="S19T1U72">error_all</span>)</span> &lt; <span class="mxinfo " id="T1:U49">0.1</span></span>)&amp;&amp;(<span class="mxinfo " id="T6:U50"><span class="var type1" id="S14T1U76">finally_flag</span> == <span class="mxinfo " id="T1:U52">0</span></span>)&amp;&amp;(<span class="mxinfo " id="T6:U53"><span class="mxinfo " id="T1:U54">norm(<span class="var type1" id="S11T3U82">v_vector</span>)</span>&lt;<span class="mxinfo " id="T1:U56">0.1</span></span>))</span></span>
<span class="srcline"><span class="lineno"><a href="1,18" id="srcline18">18</a></span><span class="line">    <span class="mxinfo " id="T1:U57"><span class="var type1" id="S13T1U86">i</span> = <span class="mxinfo " id="T1:U59"><span class="var type1" id="S13T1U88">i</span>+<span class="mxinfo " id="T1:U61">1</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,19" id="srcline19">19</a></span><span class="line">    <span class="keyword">if</span> <span class="mxinfo " id="T6:U62"><span class="var type1" id="S13T1U93">i</span> &gt; <span class="mxinfo " id="T1:U64">size(<span class="var type1" id="S6T2U96">desired</span>,1)</span></span><span class="comment">%当i超过给定轨迹点的索引时，就一直输出最后一个点的坐标</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,20" id="srcline20">20</a></span><span class="line">        <span class="mxinfo " id="T1:U66"><span class="var type1" id="S13T1U100">i</span> = <span class="mxinfo " id="T1:U68">size(<span class="var type1" id="S6T2U103">desired</span>,1)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,21" id="srcline21">21</a></span><span class="line">        <span class="mxinfo " id="T1:U70"><span class="var type1" id="S14T1U107">finally_flag</span> = <span class="mxinfo " id="T1:U72">1</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,22" id="srcline22">22</a></span><span class="line">    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,23" id="srcline23">23</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,24" id="srcline24">24</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,25" id="srcline25">25</a></span><span class="line"><span class="mxinfo " id="T1:U73"><span class="var type1" id="S2T1U111">x_d</span> = <span class="mxinfo " id="T1:U75"><span class="var type1" id="S6T2U113">desired</span>(<span class="var type1" id="S13T1U114">i</span>,<span class="mxinfo " id="T1:U78">1</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,26" id="srcline26">26</a></span><span class="line"><span class="mxinfo " id="T1:U79"><span class="var type1" id="S3T1U118">y_d</span> = <span class="mxinfo " id="T1:U81"><span class="var type1" id="S6T2U120">desired</span>(<span class="var type1" id="S13T1U121">i</span>,<span class="mxinfo " id="T1:U84">2</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,27" id="srcline27">27</a></span><span class="line"><span class="mxinfo " id="T1:U85"><span class="var type1" id="S4T1U125">z_d</span> = <span class="mxinfo " id="T1:U87"><span class="mxinfo " id="T1:U88">-<span class="mxinfo " id="T1:U89">1</span></span>*<span class="mxinfo " id="T1:U90"><span class="var type1" id="S6T2U130">desired</span>(<span class="var type1" id="S13T1U131">i</span>,<span class="mxinfo " id="T1:U93">3</span>)</span></span></span>;<span class="comment">%因为flightgear中z轴是朝下的，所以要给负值</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,28" id="srcline28">28</a></span><span class="line"><span class="var type0" id="S5T0U135">yaw_d</span> = <span class="var type1" id="S6T2U137">desired</span>(<span class="var type1" id="S13T1U138">i</span>,<span class="mxinfo " id="T1:U96"><span class="message error" id="M1F1C">4</span></span>);</span></span>
<span class="srcline"><span class="lineno"><a href="1,29" id="srcline29">29</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,30" id="srcline30">30</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,31" id="srcline31">31</a></span><span class="line"><span class="comment">% figure(1)</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,32" id="srcline32">32</a></span><span class="line"><span class="comment">% hold on </span></span></span>
<span class="srcline"><span class="lineno"><a href="1,33" id="srcline33">33</a></span><span class="line"><span class="comment">% grid on</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,34" id="srcline34">34</a></span><span class="line"><span class="comment">% plot3(act_x,act_y,-1*act_z,'ob');</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,35" id="srcline35">35</a></span><span class="line"><span class="comment">% xlabel('x');</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,36" id="srcline36">36</a></span><span class="line"><span class="comment">% ylabel('y');</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,37" id="srcline37">37</a></span><span class="line"><span class="comment">% zlabel('z');</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,38" id="srcline38">38</a></span><span class="line"><span class="comment">% plot(act_yaw);</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,39" id="srcline39">39</a></span><span class="line"><span class="keyword">end</span></span></span>
</pre>
</body>
</html>
